#include "init.h"
#include "statechanges.h"

struct ADC_t ADC;


const unsigned int * ADC_LOOKUP_FIRST[] = {&ADC.MA,&ADC.BUS,&ADC.MB,&ADC.BUS,&ADC.MC,&ADC.BUS,
                                           &ADC.MA,&ADC.BUS,&ADC.MC,&ADC.BUS,&ADC.MB,&ADC.BUS};
const unsigned int * ADC_LOOKUP_SECOND[] = {&ADC.BUS,&ADC.MC,&ADC.BUS,&ADC.MA,&ADC.BUS,&ADC.MB,
                                           &ADC.BUS,&ADC.MB,&ADC.BUS,&ADC.MA,&ADC.BUS,&ADC.MC};

void __attribute__((interrupt, no_auto_psv)) _ADCInterrupt(void)
{
    // @ 40 khz?
    ADCISR_ENTER;

    extern struct state_t state;
    extern struct setup_t setup;

    ADC.MC = ADCBUF0; 			// Read AN0 conversion result
    ADC.MB = ADCBUF2; 			// Read AN2 conversion result
    ADC.MA = ADCBUF4; 			// Read AN4 conversion result
    ADC.CURRENT = ADCBUF9; 			// Read AN4 conversion result

    if (state.BEMFmode==1)
    {
        ADC.BUS=(ADC.MA+ADC.MB+ADC.MC)/3;
    }
    else
    {
        ADC.BUS=setup.BUS_LOW;
    }
    
    IFS0bits.ADIF=0;
    
    // TODO: uitzoeken wat dit doet?
    ADSTAT=0;

    // Just ignore samples when waiting for commutation...
    if (/*state.mode < STATE_OPENLOOP || */state.mode != STATE_BEMF_SAMPLING)
    {
        ADCISR_EXIT;
        return;
    }

    ADC.debug[ADC.debugPos].MA = ADC.MA;
    ADC.debug[ADC.debugPos].MB = ADC.MB;
    ADC.debug[ADC.debugPos].MC = ADC.MC;
    ADC.debug[ADC.debugPos].step = state.step | (state.DEMAG == 0 ? 0x80 | 0x00);
    ADC.debugPos++;

    // TODO error checking
    int step = state.direction == 0 ? state.step : state.step + 6;

    // Check if demagnitized?
    if (state.DEMAG != 0)
    {
        state.DEMAG--;
        
        if (*ADC_LOOKUP_FIRST[step] < *ADC_LOOKUP_SECOND[step])
        {
            state.DEMAG = 0;
            state.debugDemag[state.step] = TMR2;
        }
        else if (state.DEMAG == 0)
        {
            state.debugDemag[state.step] = 65535;
        }

        ADCISR_EXIT;
        return;
    }

    // Wait for zero crossing...
    if (*ADC_LOOKUP_FIRST[step] < *ADC_LOOKUP_SECOND[step])
    {
        ADCISR_EXIT;
        return;
    }

    state.debugZC[state.step] = TMR2;

    // calculate zero crossing
    {
        PR2 = 65535;
        state.zerocrossAvgArray[state.step] = TMR2;
        TMR2 = 0;

        unsigned long avg = state.zerocrossAvgArray[state.step];
        int i = state.step + (state.direction == 0 ? - 1 : + 1);
        if (i < 0) i = 5;
        if (i > 5) i = 0;
        avg += state.zerocrossAvgArray[i];
        avg += state.zerocrossAvgArray[0];
        avg += state.zerocrossAvgArray[1];
        avg += state.zerocrossAvgArray[2];
        avg += state.zerocrossAvgArray[3];
        avg += state.zerocrossAvgArray[4];
        avg += state.zerocrossAvgArray[5];
        avg >>= 3;
        if (avg > 65535) avg = 65535;
        TMR3 = 0;
        PR3 = avg / 3;

        // RPMpulse = 60 degrees
        /*unsigned int t3;
        if (TMR3 < 65535 / 2)
            t3 = TMR3 * 2;
        else
            t3 = 65535;

        unsigned int avg;

        if (t3 < 65535 / 4)
            avg = PR3 / 4 + t3 * 3 / 4;
        else
            avg = PR3 / 4 + t3 / 4 * 3;

        state.RPMpulse = avg;*/
    }

    /*if (state.delay_num == 1)
        PR3 = state.RPMpulse * 2; ///state.delay_denom;
    else
    {
        long delay = state.RPMpulse*(long)state.delay_num/state.delay_denom;
        if (delay > 65535) delay = 65535;
        PR3 = delay;
    }*/
    //TMR3 = 0;

    // BEMF sampling > BEMF commutation
    stateBemfCommutationEnter();
    ADCISR_EXIT;
}
